#include "queue.h"
#include "sw_interrupt.h"
#include "semaphores.h"
#include "os_mem.h"
#include "os_io.h"
#include "user_timers.h"

static inline void wait_queue(queue_handle_t q_handle)
{
    raise_sw_interrupt(SW_INT_WAIT_QUEUE, (uint32_t)q_handle);
}

/*
 * Dequeue from the queue
*/
err_t dequeue(queue_handle_t q_handle, queue_msg_t *msg)
{
    queue_msg_t *q_msg = NULL;
    user_timer_data_t *timer_data = NULL;

    while (1) {

	if (q_handle->head.next) {
	    q_msg = q_handle->head.next;
	    q_handle->head.next = q_handle->head.next->next;
	    q_msg->next = NULL;

	    // @todo: make tail change atomic
	    if (q_handle->tail == q_msg) {
	        q_handle->tail = &q_handle->head;
	    }

	    if (q_msg->msg_id == Q_MSG_TYPE_TIMER) {
	        // Call the handler
		timer_data = (user_timer_data_t *)q_msg;

		if (timer_data->state == TIMER_CANCELLED) {
		    free((uint8_t *)timer_data);
		    continue;
		} else if (timer_data->state != TIMER_EXPIRED) {
		    printf("\nERROR!! Timer expired with invalid state 0x%x", timer_data->state);
		}

		//timer_data->user_handler(timer_data->user_handler_arg);

		// If timer is periodic, we re-submit it here
		if (timer_data->periodic) {
		   user_timer_start(timer_data);
		   if (timer_data->timer_err != ERR_SUCCESS) {
		       printf("Problem in restarting the timer\n");
		       free((uint8_t *)timer_data);
		   }

		   /*
		   * User may cancel the timer in the handler. We call the
		   * handler after re-submitting the timer so that it remains
		   * valid during re-submission
		   */

		   timer_data->user_handler(timer_data->user_handler_arg);

		} else {

		    timer_data->user_handler(timer_data->user_handler_arg);
		    free((uint8_t *)timer_data);

		}
	        continue;
	    }

	    *msg = *q_msg;
	    free((uint8_t *)q_msg);

	    return ERR_SUCCESS;
	} else {
	    wait_queue(q_handle);
	}
    }

}

err_t lookup_queue(const uint8_t *q_name, q_id_t *queue_id)
{
    queue_lookup_t ql;

    ql.q_name = q_name;
    
    raise_sw_interrupt(SW_INT_QUEUE_LOOKUP, (uint32_t)&ql);
    if (ql.err_code == ERR_SUCCESS) {
        *queue_id = ql.queue_id;
    }

    return ql.err_code;
}
    

err_t enqueue2(q_id_t queue_id, uint8_t *data, uint16_t length, queue_msg_type_t msg_id)
{
    enq_msg_t enq_msg;

    enq_msg.q_id = queue_id;
    enq_msg.msg_id = msg_id;
    enq_msg.msg_data = data;
    enq_msg.msg_len = length;
    enq_msg.err_code = ERR_SUCCESS;
    enq_msg.data_type = COPY_DATA;

    raise_sw_interrupt(SW_INT_ENQ_QUEUE, (uint32_t)&enq_msg);

    return enq_msg.err_code;
}

err_t enqueue(q_id_t queue_id, uint8_t *data, uint16_t length)
{
    return enqueue2(queue_id, data, length, Q_MSG_TYPE_IPC);
}

err_t enqueue_shared_data(q_id_t queue_id, uint8_t *data, uint16_t length)
{
    enq_msg_t enq_msg;

    enq_msg.q_id = queue_id;
    enq_msg.msg_data = data;
    enq_msg.msg_len = length;
    enq_msg.err_code = ERR_SUCCESS;
    enq_msg.data_type = SHARED_DATA;

    raise_sw_interrupt(SW_INT_ENQ_QUEUE, (uint32_t)&enq_msg);

    return enq_msg.err_code;
}


err_t create_queue(const uint8_t *queue_name, queue_options_t *options,
	queue_handle_t *handle_p)
{
    queue_handle_t q_handle; 
    err_t sem_err_code;
    err_t q_err_code;

    if (!handle_p) {
        printf("Invalid queue handle/name\n");	
	return ERR_INVALID; 
    }

    q_handle = (queue_handle_t)malloc(sizeof(queue_t));
    if (!q_handle) { return ERR_NO_MEM; }

    q_handle->q_name = queue_name;

    sem_err_code = sem_create_blocked(&q_handle->block);
    if (sem_err_code != ERR_SUCCESS) { 
        printf("Error creating queue semaphore: 0x%x\n", sem_err_code);
	free((uint8_t *)q_handle);
	return ERR_INTERNAL_ERROR;     // @todo: unify all error codes
    }

    q_handle->tail = &q_handle->head;
    q_handle->head.next = NULL;

    raise_sw_interrupt(SW_INT_CREATE_QUEUE, (uint32_t)q_handle);
    q_err_code = q_handle->err_code;

    if (q_handle->err_code != ERR_SUCCESS) {
	sem_release(q_handle->block);
	free((uint8_t *)q_handle);
    } else {
	*handle_p = q_handle;
    }

    return q_err_code;
}

err_t destroy_queue(queue_handle_t q_handle)
{
    if (!q_handle) {
	printf("Invalid queue handle being deleted\n");
	return ERR_INVALID;
    }

    q_handle->err_code = ERR_INVALID;
    raise_sw_interrupt(SW_INT_DELETE_QUEUE, (uint32_t)q_handle->queue_id);

    if (q_handle->err_code != ERR_SUCCESS) {
	printf("Error in deleting the queue\n");
	return q_handle->err_code;
    }

    sem_release(q_handle->block);
    free((uint8_t *)q_handle);

    return ERR_SUCCESS;
}
